#include <pcl/recognition/line_rgbd.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/poses_from_matches.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/vfh.h>
#include <pcl/features/boundary.h>
#include <pcl/filters/crop_box.h>
#include <pcl/segmentation/region_growing_rgb.h>
#include <pcl/segmentation/supervoxel_clustering.h>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <iostream>
#include <vector>
#include <mutex>
#include <math.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>

// Namespace
using namespace std;
mutex mutx;

typedef pcl::PointXYZRGBA PointType;
class classifier{
    private:
        cv::Ptr<cv::ml::SVM> SVMclf;
    public:
        classifier(void){
            SVMclf = cv::ml::SVM::create();
        }
        classifier(std::string filename){
            //SVMclf = cv::ml::SVM::create();
            //SVMclf->load(filename.c_str());
            SVMclf = cv::Algorithm::load<cv::ml::SVM>(filename.c_str());
        }
        void trainSVM(cv::Mat _trainingData, cv::Mat _trainingLabels){
            SVMclf->setType(cv::ml::SVM::Types::C_SVC);
            SVMclf->setKernel(cv::ml::SVM::KernelTypes::LINEAR);
            SVMclf->setTermCriteria(cv::TermCriteria(cv::TermCriteria::MAX_ITER, 100, 1e-6));

            cv::Ptr<cv::ml::TrainData> tData = cv::ml::TrainData::create(_trainingData, cv::ml::ROW_SAMPLE,_trainingLabels);
            SVMclf->train(tData);
            std::cout << "SVM trained " << std::endl;
            SVMclf->save("SVMclf->xml");
        }
        void trainSVM(cv::Mat _trainingData, cv::Mat _trainingLabels,std::string fileName){
            SVMclf->setType(cv::ml::SVM::Types::C_SVC);
            SVMclf->setKernel(cv::ml::SVM::KernelTypes::LINEAR);
            SVMclf->setTermCriteria(cv::TermCriteria(cv::TermCriteria::MAX_ITER, 100, 1e-6));

            cv::Ptr<cv::ml::TrainData> tData = cv::ml::TrainData::create(_trainingData, cv::ml::ROW_SAMPLE,_trainingLabels);
            SVMclf->train(tData);
            std::cout << "SVM trained " << std::endl;
            fileName+=".xml";
            SVMclf->save(fileName.c_str());
        }

        float getConfidence(float distance){
            float conf = 1.0 / ( 1.0 + exp(-1*distance));
            return conf;
        }
        void validateSVM(cv::Mat _testData, cv::Mat _testLabels){
            float hits = 0;
            float miss = 0;
            for(size_t idx=0;idx<_testData.cols;idx++){
                std::cout << " [test] "<< "idx:"<< idx << " " << SVMclf->predict(_testData.row(idx)) << " "
                        << _testLabels.at<float>(idx,0) << " [ confidence ] "
                        << getConfidence(SVMclf->predict(_testData.row(idx))) <<  std::endl;
                if (SVMclf->predict(_testData.row(idx))==_testLabels.at<float>(idx,0))
                    hits++;
                else
                    miss++;
            }
            printf(" [accuracy] %f ",(hits/(hits+miss)));
        }
        std::vector<float> predict(cv::Mat query){
            std::vector<float> result;
            result.push_back( SVMclf->predict(query));
            result.push_back( getConfidence(SVMclf->predict(query)));
            return result;
        }
        cv::Mat vector2Mat(pcl::PointCloud<pcl::VFHSignature308>::Ptr inputDescriptor){
            cv::Mat testArray = cv::Mat::zeros(1, 306, CV_32FC1);
            for(size_t j=0; j<306;j++){
                testArray.at<float>(0,j)= (float)inputDescriptor->points[0].histogram[j];
            }
            return testArray;
        }
};

class processPointCloud{
    private:
        float model_ss_;
    public:
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_keypoints;
        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals;
        pcl::PointCloud<pcl::VFHSignature308>::Ptr VFH_descriptor;

        pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
        pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> norm_est;

        processPointCloud(void){
            cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            cloud_keypoints = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            cloud_normals = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>);
            VFH_descriptor = pcl::PointCloud<pcl::VFHSignature308>::Ptr(new pcl::PointCloud<pcl::VFHSignature308>);
            float model_ss_ (0.001f);
        }

        void cloudinput(pcl::PointCloud<pcl::PointXYZ>::Ptr someCloud){
            cloud = someCloud;
        }

        void getNormals(void){
            norm_est.setKSearch (10);
            norm_est.setInputCloud (cloud);
            norm_est.compute (*cloud_normals);
        }

        void getKeypoints(void){
            pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
            uniform_sampling.setInputCloud (cloud);
            uniform_sampling.setRadiusSearch (model_ss_);
            uniform_sampling.filter(*cloud_keypoints);
        }

        void getVFHE(void){
            pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
            pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
            vfh.setInputCloud(cloud);
            vfh.setInputNormals(cloud_normals);
            vfh.setSearchMethod(kdtree);
            vfh.setNormalizeBins(true);
            vfh.setNormalizeDistance(false);
            vfh.compute(*VFH_descriptor);
        }

        pcl::PointCloud<pcl::VFHSignature308>::Ptr getDescriptor(void){
            getNormals();
            getKeypoints();
            getVFHE();
            return VFH_descriptor;
        }
};

float width=0.2,height=0.1,length=0.1;
pcl::PointCloud<PointType>::Ptr setflag(pcl::PointCloud<PointType>::Ptr cloud,pcl::visualization::PCLVisualizer& view)
{
    static int iflag = 0;
    pcl::PosesFromMatches pose;
    Eigen::Vector4f center,ptMin,ptMax;
    pcl::getMinMax3D(*cloud,ptMin,ptMax);
    width = abs(ptMax.x()-ptMin.x());
    height = abs(ptMax.y()-ptMin.y());
    length = abs(ptMax.z()-ptMin.z());
    pcl::compute3DCentroid(*cloud,center);

    mutx.lock();
    iflag++;
    mutx.unlock();

    Eigen::Matrix3f covariance;
    //pcl::computeMeanAndCovarianceMatrix(*cloud,covariance,center);
    pcl::computeCovarianceMatrixNormalized(*cloud,center,covariance);

    pcl::PointCloud<PointType>::Ptr final(new pcl::PointCloud<PointType>);
    pcl::PointCloud<pcl::PointNormal>::Ptr normalpt(new pcl::PointCloud<pcl::PointNormal> ());
    pcl::NormalEstimation<PointType,pcl::PointNormal> normal;
    normal.setKSearch(10);
    normal.setInputCloud(cloud);
    normal.compute(*normalpt);

    pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>());
    pcl::PointCloud<pcl::Boundary> boudary;
    pcl::BoundaryEstimation<PointType,pcl::PointNormal,pcl::Boundary> est;
    est.setInputCloud(cloud);
    est.setInputNormals(normalpt);
    est.setSearchMethod(tree);
    est.setKSearch(10);
    est.compute(boudary);

    for(int i=0;i<boudary.size();i++)
    {
        if(boudary.points[i].boundary_point==1)
        {
            cloud->points[i].g = 0xf;
            (*final).points.push_back(cloud->points[i]);
        }
    }

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();

    cout<<"quater:"<<Eigen::Quaternionf(eigenVectorsPCA).w()<<" "<<Eigen::Quaternionf(eigenVectorsPCA).x()<<" "<<Eigen::Quaternionf(eigenVectorsPCA).y()<<" "<<Eigen::Quaternionf(eigenVectorsPCA).z()<<" "<<endl;
    view.addCube(center.head<3>(),Eigen::Quaternionf(eigenVectorsPCA).Identity(),width,length,height,"cube"+iflag);
    view.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,"cube"+iflag);
    view.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cube"+iflag);
    return final;
}

pcl::PointCloud<PointType>::Ptr filter(pcl::PointCloud<PointType>::Ptr cloud,pcl::visualization::PCLVisualizer& view)
{
    //体素化
    pcl::VoxelGrid<PointType> vox;
	vox.setInputCloud(cloud);
    //vox.setRadiusSearch(0.01);
	vox.setLeafSize(0.01, 0.01, 0.01);
	vox.filter(*cloud);
    cout<<("体素化")<<endl;
    view.removeAllPointClouds();
    view.addPointCloud(cloud);
    view.updatePointCloud(cloud);
    view.spinOnce();

    //颜色过滤
    // pcl::ConditionalRemoval<PointType> condRemoval;
    // condRemoval.setInputCloud(cloud);
    // condRemoval.filter(*cloud);
    
    //去除噪声点
    // pcl::PointCloud<PointType>::Ptr sor_cloud(new pcl::PointCloud<PointType>);
    // pcl::StatisticalOutlierRemoval<PointType>sor;
	// sor.setMeanK(10);
	// sor.setInputCloud(cloud);
	// sor.setStddevMulThresh(0.02);
	// sor.filter(*sor_cloud);
    // *cloud = *sor_cloud;
    // cout<<("去除噪声点")<<endl;
    // view.removeAllPointClouds();
    // view.addPointCloud(cloud);
    // view.updatePointCloud(cloud);

    //平面分割(RANSAC)
    pcl::SACSegmentation<PointType> sac;
	pcl::PointIndices::Ptr inliner(new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointCloud<PointType>::Ptr sac_cloud(new pcl::PointCloud<PointType>);
	sac.setInputCloud(cloud);
	sac.setMethodType(pcl::SAC_RANSAC);
	sac.setModelType(pcl::SACMODEL_PLANE);
	sac.setMaxIterations(1000);
	sac.setDistanceThreshold(0.02);
    cout<<("平面分割(RANSAC)")<<endl;
    view.removeAllPointClouds();
    view.addPointCloud(cloud);
    view.updatePointCloud(cloud);
    view.spinOnce();
    //提取平面
    size_t iTotal = cloud->size();
    pcl::ExtractIndices<PointType>ext;
	pcl::PointCloud<PointType>::Ptr ext_cloud_rest(new pcl::PointCloud<PointType>);
    while (cloud->size()>iTotal*0.8)
    {
        ext.setInputCloud(cloud);
        sac.segment(*inliner, *coefficients);
        if (inliner->indices.size()==0)
			break;
        ext.setIndices(inliner);
		ext.setNegative(true);
		ext.filter(*ext_cloud_rest);
        *cloud = *ext_cloud_rest;
    }

    cout<<("提取平面")<<endl;
    view.removeAllPointClouds();
    view.addPointCloud(cloud);
    view.updatePointCloud(cloud);
    view.spinOnce();

    //欧式聚类
	vector<pcl::PointIndices>ece_inlier;
	pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
	pcl::EuclideanClusterExtraction<PointType> ece;
	ece.setInputCloud(cloud);
	ece.setClusterTolerance(0.02);
	ece.setMinClusterSize(30);
	ece.setMaxClusterSize(cloud->points.size());
	ece.setSearchMethod(tree);
	ece.extract(ece_inlier);
    pcl::PointCloud<PointType>::Ptr cloud_dest(new pcl::PointCloud<PointType>);
    cout<<"欧式聚类分割"<<endl;

    pcl::PointCloud<PointType>::Ptr final(new pcl::PointCloud<PointType>);
    processPointCloud pCloud;
    classifier classify("./bin/zhuantou.xml");
    for (int i = 0; i < ece_inlier.size();i++)
    {
        vector<int> ece_inlier_ext = ece_inlier[i].indices;
        pcl::PointCloud<PointType>::Ptr cloud_copy(new pcl::PointCloud<PointType>);
        pcl::copyPointCloud(*cloud, ece_inlier_ext, *cloud_copy);
        //if(cloud_copy->points.size()>105||cloud_copy->points.size()<95)
        //   continue;
        cout<<"class "<<i<<"："<<cloud_copy->points.size()<<endl;
        //////////////////////////////////////////////
        pcl::PointCloud<pcl::PointXYZ>::Ptr dst(new pcl::PointCloud<pcl::PointXYZ>);
        (*dst).width = (*cloud_copy).width;
        (*dst).height = (*cloud_copy).height;
        for(int j=0;j<(*cloud_copy).size();j++)
        {
            (*dst).points.push_back(pcl::PointXYZ(
                (*cloud_copy).points[j].x,
                (*cloud_copy).points[j].y,
                (*cloud_copy).points[j].z
            ));
        }

        pCloud.cloudinput(dst);
        cv::Mat testArray = cv::Mat::zeros(1, 306, CV_32FC1);
        pcl::PointCloud<pcl::VFHSignature308>::Ptr inputDescriptor = pCloud.getDescriptor();
        for(int k=0;k<306;k++)
            testArray.at<float>(0,k) = (float)inputDescriptor->points[0].histogram[k];
        std::vector<float> res = classify.predict(testArray);
        cout<<"predict:"<<res[0]<<"  "<<res[1]<<endl;
        *cloud_dest += *cloud_copy;
        if(res[1]==1 && res[0]>0.8)
            *final += *setflag(cloud_copy,view);
        if(i==6)            
            pcl::io::savePLYFile("./assets/zhuantou.ply",(*cloud_copy));
        //////////////////////////////////////////////
        
        view.removeAllPointClouds();
        view.addPointCloud(cloud_dest);
        view.updatePointCloud(cloud_dest);
        view.spinOnce();

        sleep(2);
    }
    cout<<"欧式聚类:"<<ece_inlier.size()<<"个类别"<<endl;
    *cloud = *cloud_dest;
    return final;
}

int main(int argc, char **argv)
{
    cout<<("the begin")<<endl;
    pcl::PointCloud<PointType>::Ptr cloudbk(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
    //pcl::visualization::CloudViewer cloudView("test");
    pcl::visualization::PCLVisualizer cloudView2("test");
    cloudView2.setBackgroundColor(0,0,0);
    if(string(argv[1]).find(".ply")==string::npos)
    {
        pcl::io::loadPCDFile(argv[1], *cloud);
    }
    else
    {
        pcl::io::loadPLYFile(argv[1], *cloud);
    }

    pcl::copyPointCloud(*cloud,*cloudbk);
    cout<<("the load finish")<<endl;
    *cloudbk += *filter(cloud,cloudView2);
    //cloudView.showCloud(cloud,"test");
    
    cout<<("the end")<<endl;
    while (!cloudView2.wasStopped())
        cloudView2.spin();
    return 0;
}